#include "lis302dl.h"

/* Read/Write command */
#define READWRITE_CMD              ((uint8_t)0x80)
/* Multiple byte read/write command */
#define MULTIPLEBYTE_CMD           ((uint8_t)0x40)

const uint8_t lis_dummy_tx[12] = {
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
};

uint8_t lis_rx_buf[8] = {0};

uint8_t lis302dl_write_reg(LIS302DL_HandleTypeDef* lis302dl,
			  uint8_t reg_addr,
			  uint8_t reg_data);
uint8_t lis302dl_read_reg(LIS302DL_HandleTypeDef* lis302dl, uint8_t reg_addr);
void lis302dl_read_regs(LIS302DL_HandleTypeDef* lis302dl,
		       uint8_t reg_addr,
		       uint8_t* data,
		       uint8_t len);
uint8_t lis302dl_nonblocking_init(LIS302DL_HandleTypeDef* lis);
void lis302dl_handle_byte(void* pLis);

void lis302dl_nonblocking_process(LIS302DL_HandleTypeDef* lis)
{
	switch (lis->status) {
	case LIS_INIT:
		if (LIS_INIT_COMPLETE == lis302dl_nonblocking_init(lis))
			lis->status = LIS_CHECK_FS;
		return;
	case LIS_CHECK_FS:
		lis->initStructure.Full_Scale =
			lis302dl_read_reg(lis, LIS302DL_CTRL_REG1_ADDR) & 0x20;
		lis->status = LIS_READ_ACC;
		return;
	case LIS_READ_ACC: {
		uint8_t cmd = LIS302DL_OUT_X_ADDR | READWRITE_CMD |
			MULTIPLEBYTE_CMD;
		lis->p_imu = (struct imu_raw_data *)spmc_malloc(0);
		if (lis->p_imu == NULL)
			return;
		spi_select(LIS302DL_CS_PIN, 1);
		HAL_SPI_Transmit(lis->spi, &cmd, 1, 1000);
		lis->p_imu->timestamp = board_get_systime();
		lis->rx_handler = lis302dl_handle_byte;
		HAL_SPI_TransmitReceive_DMA(lis->spi, lis_dummy_tx,
					    lis->rx_byte, 6);
		}
		return;
	case LIS_SET_FS:
		lis->status = LIS_READ_ACC;
	default:
		return;
	}
}

uint8_t lis302dl_read_pid(LIS302DL_HandleTypeDef* lis)
{
	uint8_t response = 0;
	response = lis302dl_write_reg(lis,
				      LIS302DL_WHO_AM_I_ADDR|READWRITE_CMD,
				      0x00);
	return response;
}

uint8_t lis302dl_write_reg(LIS302DL_HandleTypeDef* lis,
			  uint8_t reg_addr,
			  uint8_t reg_data)
{
	uint8_t temp_val = 0;
	uint8_t _reg_addr = reg_addr;
	uint8_t _reg_data = reg_data;
	spi_select(LIS302DL_CS_PIN, 1);
	HAL_SPI_Transmit(lis->spi, &_reg_addr, 1, 1000);
	HAL_SPI_TransmitReceive(lis->spi, &_reg_data, &temp_val, 1, 1000);
	spi_select(LIS302DL_CS_PIN, 0);
	return temp_val;
}

uint8_t lis302dl_read_reg(LIS302DL_HandleTypeDef* lis, uint8_t reg_addr)
{
	return lis302dl_write_reg(lis, reg_addr | READWRITE_CMD, 0xff);
}

void lis302dl_read_regs(LIS302DL_HandleTypeDef* lis,
		       uint8_t reg_addr,
		       uint8_t* data,
		       uint8_t len)
{
	uint8_t address = reg_addr | READWRITE_CMD | MULTIPLEBYTE_CMD;
	spi_select(LIS302DL_CS_PIN, 1);
	HAL_SPI_Transmit(lis->spi, &address, 1, 1000);
	HAL_SPI_Receive(lis->spi, data, len, 1000);
	spi_select(LIS302DL_CS_PIN, 0);
}

uint8_t lis302dl_nonblocking_init(LIS302DL_HandleTypeDef* lis)
{
	static uint8_t delay_cnt = 0;

	switch (lis->initStructure.status) {
	case LIS_UNIDENTIFIED:
		if (delay_cnt++ > lis->init_delay) {
			delay_cnt = 0;
			uint8_t pid = lis302dl_read_pid(lis);
			if (0x3b == pid) {
				lis->initStructure.status = LIS_INIT_REG1;
			}
		}
		return LIS_UNIDENTIFIED;
	case LIS_INIT_REG1: {
		if (delay_cnt++ == 0) {
			lis->rx_byte = lis_rx_buf;
			uint8_t ctrl = lis->initStructure.Output_DataRate |
				lis->initStructure.Power_Mode |
				lis->initStructure.Full_Scale |
				lis->initStructure.Self_Test |
				lis->initStructure.Axes_Enable;
			lis302dl_write_reg(lis, LIS302DL_CTRL_REG1_ADDR, ctrl);
		} else if (delay_cnt++ > lis->init_delay) {
			delay_cnt = 0;
			lis->initStructure.status = LIS_INIT_COMPLETE;
		}
		return LIS_INIT_REG1;
	}
	case LIS_INIT_COMPLETE:
		return LIS_INIT_COMPLETE;
	default:
		lis->initStructure.status = LIS_INIT_REG1;
		return LIS_INIT_REG1;
	}
}

void lis302dl_handle_byte(void *pLis)
{
	LIS302DL_HandleTypeDef *lis = (LIS302DL_HandleTypeDef *)pLis;
	float scale = 1.0;
	if (lis->initStructure.Full_Scale == LIS302DL_FULLSCALE_2_3) {
		scale = LIS302DL_SENSITIVITY_2_3G;
	} else if (lis->initStructure.Full_Scale == LIS302DL_FULLSCALE_9_2) {
		scale = LIS302DL_SENSITIVITY_9_2G;
	}
	lis->p_imu->acc[0] = ((int8_t)lis->rx_byte[0]) *
		scale * GRAVITY;
	lis->p_imu->acc[1] = ((int8_t)lis->rx_byte[2]) *
		scale * GRAVITY;
	lis->p_imu->acc[2] = ((int8_t)lis->rx_byte[4]) *
		scale * GRAVITY;
	event_post(&event_queue, EVENT_IMU_DATA, (void *)(lis->p_imu));
}
